#include "KinectReader.h"

// initializing singleton
KinectReader * KinectReader::instance = 0;
bool tracking = false;
int ignoredReadings[KinectReader::MAX_USERS];
bool readingsInitialized = false;



// Callback: New user was detected
void XN_CALLBACK_TYPE User_NewUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
	printf("New User %d\n", nId);
	
	KinectReader::getInstance()->getUserGenerator()->GetSkeletonCap().RequestCalibration(nId, TRUE);

	KinectReader::getInstance()->getSkeletons()[nId].setActive(true);
}

// Callback: An existing user was lost
void XN_CALLBACK_TYPE User_LostUser(xn::UserGenerator& generator, XnUserID nId, void* pCookie) {
	printf("Lost user %d\n", nId);

	KinectReader::getInstance()->getSkeletons()[nId].setActive(false);
}


// Callback: Detected a pose
void XN_CALLBACK_TYPE UserPose_PoseDetected(xn::PoseDetectionCapability& capability, const XnChar* strPose, XnUserID nId, void* pCookie) {
	printf("Pose %s detected for user %d\n", strPose, nId);
	KinectReader::getInstance()->getUserGenerator()->GetPoseDetectionCap().StopPoseDetection(nId);
	KinectReader::getInstance()->getUserGenerator()->GetSkeletonCap().RequestCalibration(nId, TRUE);
}


// Callback: Started calibration
void XN_CALLBACK_TYPE UserCalibration_CalibrationStart(xn::SkeletonCapability& capability, XnUserID nId, void* pCookie) {
	printf("Calibration started for user %d\n", nId);
}

// Callback: Finished calibration
void XN_CALLBACK_TYPE UserCalibration_CalibrationEnd(xn::SkeletonCapability& capability, XnUserID nId, XnBool bSuccess, void* pCookie) {
	if (bSuccess) {
		printf("Calibration complete, start tracking user %d\n", nId);
		KinectReader::getInstance()->getUserGenerator()->GetSkeletonCap().StartTracking(nId);
	}
	else {
		printf("Calibration failed for user %d\n", nId);
		KinectReader::getInstance()->getUserGenerator()->GetSkeletonCap().RequestCalibration(nId, TRUE);
	}
}



KinectReader::KinectReader(bool mirrorMode)
{

	mult_x = 1;
	mult_y = 1;
	mult_z = 1;

	off_x = 0.0;
	off_y = 0.0;
	off_z = 0.0;


	initSkeletons();

	this->mirrorMode = mirrorMode;

	this->userGenerator = new xn::UserGenerator();

	this->context = new xn::Context();
	this->depth = new xn::DepthGenerator();

	this->context->Init();
	this->depth->Create(*context);


	mapMode = new XnMapOutputMode();
	mapMode->nXRes = XN_VGA_X_RES;
	mapMode->nYRes = XN_VGA_Y_RES;
	mapMode->nFPS = 30;
	this->depth->SetMapOutputMode(*mapMode);

	XnStatus nRetVal = XN_STATUS_OK;
	nRetVal = context->FindExistingNode(XN_NODE_TYPE_USER, *(this->userGenerator));
	if (nRetVal != XN_STATUS_OK)
		nRetVal = this->userGenerator->Create(*context);

	XnCallbackHandle hUserCallbacks, hCalibrationCallbacks, hPoseCallbacks;
	checkRetVal(userGenerator->RegisterUserCallbacks(User_NewUser, User_LostUser, NULL, hUserCallbacks));
	checkRetVal(userGenerator->GetSkeletonCap().RegisterCalibrationCallbacks(UserCalibration_CalibrationStart, UserCalibration_CalibrationEnd, NULL, hCalibrationCallbacks));
	checkRetVal(userGenerator->GetPoseDetectionCap().RegisterToPoseCallbacks(UserPose_PoseDetected, NULL, NULL, hPoseCallbacks));
	checkRetVal(userGenerator->GetSkeletonCap().GetCalibrationPose(g_strPose));
	checkRetVal(userGenerator->GetSkeletonCap().SetSkeletonProfile(XN_SKEL_PROFILE_ALL));
	xnSetMirror(*depth, mirrorMode);


	printf("Initialized Kinect, looking for users...\n\n");
	context->StartGeneratingAll();


}

KinectReader::~KinectReader(void)
{
	
	context->Shutdown();
	
	delete this->userGenerator;

	delete this->context;
	delete this->depth;
}


void KinectReader::initSkeletons() {
	for(int i = 0; i < KinectReader::MAX_USERS; i++) {
		this->skeletons[i].setActive(false);
	}
}


void KinectReader::checkRetVal(XnStatus nRetVal) {
	if (nRetVal != XN_STATUS_OK) {
		printf("There was a problem initializing kinect... Make sure you have\
connected both usb and power cables and that the driver and OpenNI framework\
are correctly installed.\n\n");
		exit(1);
	}
}

void KinectReader::updateSkeletonData() {
	// Read next available data
	context->WaitAnyUpdateAll();
	// Process the data
	xn::DepthMetaData depthMD;
	depth->GetMetaData(depthMD);

	XnUserID aUsers[KinectReader::MAX_USERS];
	XnUInt16 nUsers = KinectReader::MAX_USERS;
	userGenerator->GetUsers(aUsers, nUsers);
	for (int i = 0; i < nUsers; ++i)
	{
		if (userGenerator->GetSkeletonCap().IsTracking(aUsers[i])) {
			
			if(!tracking) {
				printf("Sleeping for 5 seconds for T pos...\n");
				Sleep(5000);
				tracking = true;
				printf("Done.\n");
			}

			if(!readingsInitialized){
				for(int i = 0; i < KinectReader::MAX_USERS; i++) {
					ignoredReadings[i] = 0;
				}
				readingsInitialized = true;
			}

			if (jointPos(aUsers[i], XN_SKEL_HEAD) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}

				this->skeletons[aUsers[i]].getHead()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getHead()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getHead()->getPosition()->setZ(this->jointCoords[3]);

			}
			if (jointPos(aUsers[i], XN_SKEL_NECK) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getNeck()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getNeck()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getNeck()->getPosition()->setZ(this->jointCoords[3]);

			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_COLLAR) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getLCollar()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLCollar()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLCollar()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_SHOULDER) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				
				this->skeletons[aUsers[i]].getLShoulder()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLShoulder()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLShoulder()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_ELBOW) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getLElbow()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLElbow()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLElbow()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_WRIST) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getLWrist()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLWrist()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLWrist()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_HAND) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getLHand()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLHand()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLHand()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_FINGERTIP) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getLFinger()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLFinger()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLFinger()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_COLLAR) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRCollar()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRCollar()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRCollar()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_SHOULDER) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRShoulder()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRShoulder()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRShoulder()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_ELBOW) == 0 && tracking) {

				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRElbow()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRElbow()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRElbow()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_WRIST) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRWrist()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRWrist()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRWrist()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_HAND) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRHand()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRHand()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRHand()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_FINGERTIP) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRFinger()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRFinger()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRFinger()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_TORSO) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getTorso()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getTorso()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getTorso()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_WAIST) == 0 && tracking) {
				
				printf("Updating waist position....");

				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getWaist()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getWaist()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getWaist()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_HIP) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getLHip()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLHip()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLHip()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_KNEE) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getLKnee()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLKnee()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLKnee()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_ANKLE) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getLAnkle()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLAnkle()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLAnkle()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_LEFT_FOOT) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getLFoot()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getLFoot()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getLFoot()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_HIP) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRHip()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRHip()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRHip()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_KNEE) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRKnee()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRKnee()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRKnee()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_ANKLE) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRAnkle()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRAnkle()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRAnkle()->getPosition()->setZ(this->jointCoords[3]);
			}
			if (jointPos(aUsers[i], XN_SKEL_RIGHT_FOOT) == 0 && tracking) {
				
				if(ignoredReadings[i] < MAX_IGNORED_READINGS) {
					ignoredReadings[i]++;
					continue;
				}
				
				this->skeletons[aUsers[i]].getRFoot()->getPosition()->setX(this->jointCoords[1]);
				this->skeletons[aUsers[i]].getRFoot()->getPosition()->setY(this->jointCoords[2]);
				this->skeletons[aUsers[i]].getRFoot()->getPosition()->setZ(this->jointCoords[3]);
			}

			

		}
	}

}


int KinectReader::jointPos(XnUserID player, XnSkeletonJoint eJoint) {
	XnSkeletonJointPosition joint;
	userGenerator->GetSkeletonCap().GetSkeletonJointPosition(player, eJoint, joint);

	if (joint.fConfidence < 0.5)
		return -1;


	// printf("joint %d: (%f, %f, %f)\n", player, joint.position.X, joint.position.Y, joint.position.Z);

	jointCoords[0] = player;
	jointCoords[1] = 1.0* (off_x + (mult_x * joint.position.X / 1280)); //Normalize coords to 0..1 interval
	jointCoords[2] = -2.0* (off_y + (mult_y * (1280 - joint.position.Y) / 2560)); //Normalize coords to 0..1 interval
	jointCoords[3] = -1.0*(off_z + (mult_z * joint.position.Z * 7.8125 / 10000)); //Normalize coords to 0..7.8125 interval
	return 0;
}